# -- coding:utf-8 --
#!/usr/bin/python
import sys, os
sys.path.append("/home/avatar/Minibot/04.RobotSamples/01.Arm")
import inspect
from LoadJson import LoadJson
from BwSensorLib import BwSensorLib
from ArmRobot import ArmRobot
from BwRobotLib import BwRobotLib
from MiniBotControlLib import MiniBotControlLib
import time
import BwHelpLib 

# 加载机器人构型配置
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath = os.path.dirname(os.path.abspath(fileName))
config_file = folderPath+"/config.txt"


class ColorSorting:
    def __init__(self):
        # 实例化MiniBotControlLib类，并设置为udp模式
        self.mlib = MiniBotControlLib("udp")
        # 输入ip地址和端口号
        self.ip = "10.10.100.254"
        self.port = 8899
        # 连接机器人,并设置超时时间2s
        self.mlib.Connect(self.ip, self.port, 2000)
        # 实例化BwRobotLib类，并将mlib作为参数传入
        self.robotlib = BwRobotLib(self.mlib)
        # 导入配置文件，获取文件中的模块ID和模块连接位置
        self.module_ids, self.module_ids_loc = self.robotlib.loadConfig(config_file)
        # 扫描当前机器人构型的ID，默认扫描ID段为20-50
        self.module_ids_scan, self.module_ids_loc_scan = self.mlib.ScanId()
        # 判断是否扫描的ID与配置文件导入的ID一致
        res = BwHelpLib.checkListEqual(self.module_ids,self.module_ids_scan)
        if res == False:
            print("\033[31m输入的模块ID与扫描获取的模块ID不一致，请检查！\033[37m")
            print("配置文件中模块ID: ", self.module_ids)
            print("实际扫描的模块ID: ", self.module_ids_scan)
            exit(0)
        else:
            print("输入的模块ID与扫描获取的模块ID一致")
            

        # 实例化操作臂类
        self.armlib = ArmRobot(self.mlib)
        # 将模块ID与操作臂构型绑定
        self.armlib.bindIds(self.module_ids)        
        # 实例化传感器库对象
        self.sensorlib = BwSensorLib(self.mlib)
        # 动作延时时间
        self.wait_time = 1.5
        # 动作速度
        self.speed = 50
        # RGB彩灯参数
        # [255,1,1] 红色；[1,255,1] 绿色；[1,1,255] 蓝色；[1,1,1] 熄灯；
        self.rgb_value = [[255, 1, 1], [1, 255, 1], [1, 1, 255], [1, 1, 1]]

        # 动作组数据
        self.ZoneAToSensor = []
        self.ZoneBToSensor = []
        self.ZoneCToSensor = []
        self.SensorToRed = []
        self.SensorToGreen = []
        self.SensorToBlue = []

        # 实例化导入动作组数据对象
        self.load_json = LoadJson()
        # A点到传感器点的运动序列
        self.ZoneAToSensor = self.load_json.LoadActionDataAToSensor(
            self.ZoneAToSensor)
        # B点到传感器点的运动序列
        self.ZoneBToSensor = self.load_json.LoadActionDataBToSensor(
            self.ZoneBToSensor)
        # C点到传感器点的运动序列
        self.ZoneCToSensor = self.load_json.LoadActionDataCToSensor(
            self.ZoneCToSensor)
        # 传感器点到红色点的运动序列
        self.SensorToRed = self.load_json.LoadActionDataSensorToRed(
            self.SensorToRed)
        # 传感器点到绿色点的运动序列
        self.SensorToGreen = self.load_json.LoadActionDataSensorToGreen(
            self.SensorToGreen)
        # 传感器点到蓝色点的运动序列
        self.SensorToBlue = self.load_json.LoadActionDataSensorToBlue(
            self.SensorToBlue)
        # 初始化动作
        self.initRobotMove()

    # 初始化至实验开始位置
    def initRobotMove(self, _time=2):
        angs = [-90, 38.6, 65.9, 63, 1.2, 1.1]
        speed = 20
        self.armlib.position_angles = angs
        self.armlib.pushJointsAngles(speed, self.wait_time)
        time.sleep(_time)

    # 颜色分拣
    def sorting(self):
        # 1.操作臂夹取物体从开始区至传感器区进行颜色识别
        for a in range(3):
            # 第一次抓取方块，从最左边位置开始抓取
            if (a == 0):
                color = -1  # 初始化颜色变量
                for b in range(8):  # 从初始位置到夹方块有8组动作
                    # 从A点到传感器区域抓取方块
                    self.armlib.position_angles = self.ZoneAToSensor[b]
                    self.armlib.pushJointsAngles(self.speed, self.wait_time)
                    if(b == 7):  # 机械臂转至传感器区域，动作幅度较大，设置等待时间
                        time.sleep(self.wait_time)
                        while (color == -1):  # 通过颜色传感器检测方块颜色，直到检测到颜色为止，1：红色，2：绿色 3：蓝色
                            color = self.sensorlib.readColorSensor()
                            time.sleep(0.1)

            # 第二次抓取方块，从最中间位置开始抓取
            elif (a == 1):
                color = -1  # 初始化颜色变量
                for b in range(7):  # 从初始位置到夹方块有7组动作
                    # 从B点到传感器区域抓取方块
                    self.armlib.position_angles = self.ZoneBToSensor[b]
                    self.armlib.pushJointsAngles(self.speed, self.wait_time)
                    if(b == 6): # 机械臂转至传感器区域，动作幅度较大，设置等待时间
                        time.sleep(self.wait_time)
                        while (color == -1): # 通过颜色传感器检测方块颜色，直到检测到颜色为止，1：红色，2：绿色 3：蓝色
                            color = self.sensorlib.readColorSensor()
                            time.sleep(0.1)
            # 第三次抓取方块，从最右边位置开始抓取
            else:
                color = -1
                for b in range(8): # 从初始位置到夹方块有8组动作
                    # 从C点到传感器区域抓取方块
                    self.armlib.position_angles = self.ZoneCToSensor[b]
                    self.armlib.pushJointsAngles(self.speed, self.wait_time)
                    if(b == 7): # 机械臂转至传感器区域，动作幅度较大，设置等待时间
                        time.sleep(self.wait_time)
                        while (color == -1): # 通过颜色传感器检测方块颜色，直到检测到颜色为止，1：红色，2：绿色 3：蓝色
                            color = self.sensorlib.readColorSensor()
                            time.sleep(0.1)

            # 2.颜色识别后，操作臂将方块抓取至对应的放置区
            if(color == 1):
                print("检测到红色")
                self.sensorlib.setRGBLightSensor(
                    self.rgb_value[0])  # RBG灯设置成红色
                for b in range(0, 5):  # 从传感器区移动至红色放置区
                    self.armlib.position_angles = self.SensorToRed[b]
                    self.armlib.pushJointsAngles(self.speed, self.wait_time)
                    if(b == 2): # 机械臂转至传感器区域，动作幅度较大，设置等待时间
                        time.sleep(self.wait_time)

            elif(color == 2):
                print("检测到绿色")
                self.sensorlib.setRGBLightSensor(
                    self.rgb_value[1])  # RBG灯设置成绿色
                for b in range(0, 5):  # 从传感器区移动至绿色放置区
                    self.armlib.position_angles = self.SensorToGreen[b]
                    self.armlib.pushJointsAngles(self.speed, self.wait_time)
                    if(b == 2): # 机械臂转至传感器区域，动作幅度较大，设置等待时间
                        time.sleep(self.wait_time)

            elif(color == 3):
                print("检测到蓝色")
                self.sensorlib.setRGBLightSensor(
                    self.rgb_value[2]) # RBG灯设置成绿色
                for b in range(0, 5): # 从传感器区移动至蓝色放置区
                    self.armlib.position_angles = self.SensorToBlue[b]
                    self.armlib.pushJointsAngles(self.speed, self.wait_time)
                    if(b == 2): # 机械臂转至传感器区域，动作幅度较大，设置等待时间
                        time.sleep(self.wait_time)

            # 如果没有检测到颜色，则返回初始动作，rgb灯熄灭
            else:
                print("没检测到颜色")
                self.sensorlib.setRGBLightSensor(
                    self.rgb_value[8]) # RBG灯设置成熄灭
                self.initRobotMove(1) # 机械臂回归初始化位置

# 主函数
def main():
    # 实例化颜色分拣类
    color = ColorSorting()
    # 执行分拣功能3次
    for i in range(3):
        color.sorting()


if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        print(" Program closed! ")
    pass
